/*
 * 	MagicSmokeMachine.c
 *
 *	Description: The top level state machine. This state 
 *	machine controls the progression through coarse alignment, 
 * 	fine alignment, and charging. 
 *
 *  Created on: Oct 29, 2012
 *      Author: banfield
 */

/*****************************************************
 * Include Statements
 ****************************************************/
#include "hw_types.h"
#include "hw_ints.h"
#include "MagicSmokeMachine.h"
#include "timer.h"
#include "hw_memmap.h"
#include "gpio.h"
#include "interrupt.h"
#include "sysctl.h"
#include "systick.h"
#include "URF.h"
#include <stdlib.h>
#include "CoarseAlignmentSM.h"
#include "FineAlignmentSM.h"
#include <stdio.h>
#include "MotorControl.h"

/***************************************************
 * Module prototypes
 */
void TurnOffIndictorLEDs(void);

/*****************************************************
 * Define Statements
 ****************************************************/
#define	URF_THRESHOLD 100		//trigger distance from URF (in cm) that indicates a car has arrived
#define	URF_MARGIN 3			//buffer distance for URF noise, in cm
#define	PARKING_TIME (5*TICKS_PER_SEC) // time that a car must remain stationary before alignment begins 
#define	PARKED_TIME (3*TICKS_PER_SEC)

#define LED_BASE GPIO_PORTH_BASE // port for indicator LEDs
#define POWER_LED GPIO_PIN_0 	
#define CAR_PRESENT_LED GPIO_PIN_1
#define WITHIN_RANGE_LED GPIO_PIN_2
#define ALIGNED_LED GPIO_PIN_3
#define LED_PERIPH SYSCTL_PERIPH_GPIOH
/*****************************************************
 * Module Level Variables
 ****************************************************/
static MSMState_t currentState;	// Module level variable contains the current state
int URFval;

/*****************************************************
 * Function: InitMSM
 *
 * Description: Initialize all necessary ports and 
 * peripherals
 ****************************************************/
void InitMSM(void){
	puts("Initializing MSM state machine.");

	// All initialization connected to this SM goes here
	
	SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);

	// Set up a Timer
	TimerConfigure(TIMER1_BASE,TIMER_CFG_32_BIT_OS);
	TimerIntEnable(TIMER1_BASE,TIMER_TIMA_TIMEOUT);
	IntEnable(INT_TIMER1A);

	/*************** set ENABLE pins ******************/

	SysCtlPeripheralEnable(LED_PERIPH);
	GPIODirModeSet(LED_BASE, POWER_LED|CAR_PRESENT_LED|WITHIN_RANGE_LED|ALIGNED_LED, GPIO_DIR_MODE_OUT);
	GPIOPadConfigSet(LED_BASE, POWER_LED|CAR_PRESENT_LED|WITHIN_RANGE_LED|ALIGNED_LED, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);


	// enable processor interrupts (yeah...)
	IntMasterEnable();

	//ReturnHome();

	currentState = WaitingForCar;
	TurnOffIndictorLEDs();
	GPIOPinWrite(LED_BASE, POWER_LED, 0xFF); // Indicate power on
	//currentState = FineAlignment; // For testing purposes
}

/*****************************************************
 * Function: RunMSM
 *
 * Description: Run the Magic Smoke SM 
 ****************************************************/
void RunMSM(void){
	// if the car leaves set currentState to WaitingForCar
	int URFval = getURFRange();
	if(URFval > URF_THRESHOLD && currentState != CoarseAlignment){
		currentState = WaitingForCar;
	}

	// execute one of the following cases based on the value of
	// currentState
	switch(currentState)
	{
		case WaitingForCar:
		{
			/* 
			*	The URF sensor is used to detect the presence of a car.
			*	The car must remain stationary for PARKING_TIME before 
			*	alignment begins
			*/
			static int lastVal = 0;
			if(URFval < URF_THRESHOLD){
				if((abs(lastVal - URFval))> URF_MARGIN){
					// Timer 1 is started when a car arrives and restarted 
					// if the car moves
					TimerLoadSet(TIMER1_BASE,TIMER_A, PARKING_TIME);
					TimerEnable(TIMER1_BASE,TIMER_A);

					// turn on the car present LED
					TurnOffIndictorLEDs();
					GPIOPinWrite(LED_BASE, CAR_PRESENT_LED, 0xFF);
					
					printf("Detected stationary car. Range = %i\n", URFval);
				}
			}else{
			
				// if the car leaves the timer is disabled
				TimerDisable(TIMER1_BASE, TIMER_A);
			}

			lastVal = URFval;
		}
		break;
		case CoarseAlignment:
		{
			//call the run function for the nested URF state machine
			char alignmentState = RunCoarseAlignmentSM();

			if(alignmentState > 0){
				//if alignmentState is positive, alignment is complete
				TurnOffIndictorLEDs();
				GPIOPinWrite(LED_BASE, WITHIN_RANGE_LED, 0xFF);
				currentState = FineAlignment;
				for(;;);
			}else if(alignmentState < 0){
				//if alignmentState is negative, alignment impossible, notify driver
				//TODO: notify driver
				currentState = WaitingForDeparture;
			}
		}
		break;
		case FineAlignment:
		{
			//call the run function for the nested RFID state machine
 			if(RunFineAlignmentSM()){
				//if the run function returns a nonzero value alignment is complete
 				TurnOffIndictorLEDs();
 				GPIOPinWrite(LED_BASE, ALIGNED_LED, 0xFF);
				currentState = Charging;
			}
		}
		break;
		case Charging:
		{
			//TODO: get charging state from wall unit over CAN
			currentState = WaitingForDeparture;
		}
		break;
		case WaitingForDeparture:
		{
			//wait in this state until the car leaves the parking spot
		}
	}
}

// TIMER1A is used to make sure that the car remains stationary
void TIMER1A_Int_Handler(void){
	TimerIntClear(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
	
	// If the timer runs out, coarse alignment is started
	currentState = CoarseAlignment; 
}

void TurnOffIndictorLEDs(void){
	// TURN OFF 3 INDICATOR LEDS
	GPIOPinWrite(LED_BASE, CAR_PRESENT_LED|WITHIN_RANGE_LED|ALIGNED_LED, 0x00);
}
